
__device__ void crossv3(float v0[3], float v1[3], float re[3])
{
	re[0] = v0[1] * v1[2] - v1[1] * v0[2];
	re[1] = v1[0] * v0[2] - v0[0] * v1[2];
	re[2] = v0[0] * v1[1] - v1[0] * v0[1];
}

__device__ float vecLen2(float v[3])
{
	return v[0]*v[0]+v[1]*v[1]+v[2]*v[2];
}

__device__ void vecMinus(float v0[3], float v1[3], float re[3])
{
	re[0] = v0[0]-v1[0];
	re[1] = v0[1]-v1[1];
	re[2] = v0[2]-v1[2];
}

__device__ float vecDot(float v0[3], float v1[3])
{
	return v0[0]*v1[0]+v0[1]*v1[1]+v0[2]*v1[2];
}

__device__ void BaryCoor(float pt[3], float A[3], float B[3], float C[3], float Bary[3])
{
	// Compute vectors        
	float v0[3], v1[3], v2[3];
	vecMinus(C, A, v0);
	vecMinus(B, A, v1);
	vecMinus(pt, A, v2);

	// Compute dot products
	float dot00 = vecDot(v0, v0);
	float dot01 = vecDot(v0, v1);
	float dot02 = vecDot(v0, v2);
	float dot11 = vecDot(v1, v1);
	float dot12 = vecDot(v1, v2);

	// Compute barycentric coordinates
	float invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
	Bary[0] = (dot11 * dot02 - dot01 * dot12) * invDenom;
	Bary[1] = (dot00 * dot12 - dot01 * dot02) * invDenom;
	Bary[2] = 1.0-Bary[0]-Bary[1];
}

__device__ bool ptInTri(float pt[3], float A[3], float B[3], float C[3])
{
	float bary[3];
	BaryCoor(pt, A, B, C, bary);
	return ((bary[0] >= 0) && (bary[1] >= 0) && (bary[2] >= 0));
}

__device__ bool checkGround(float pt[3])
{
	float A[3] = {-3,-3,-3};
	float B[3] = {3,-3,-3};
	float C[3] = {-3,-3,3};
	//float A[3] = {4.0032053,6.0981870,3.9446042};
	//float B[3] = {3.9446270,6.0981870,4.0031824};
	////float C[3] = {3.9046602,6.0522494,3.9046512};
	//float C[3] = {3.5234351,5.4613929,3.5234160};
	return ptInTri(pt, A, B, C);
}


//void FitPlane2(float P0[3], float P1[3], float P2[3], float N[3])
//{
//	//three point P0,P1,P2 determined a plane with normal N;
//	//point are in homogeneous coordinate;
//	//plane equation Ax+By+Cz+D=0; D = 1;
//	N[0] = P0[1]*(P1[2] - P2[2]) + P1[1] * (P2[2] - P0[2]) + P2[1] * (P0[2] - P1[2]); 
//	N[1] = P0[2]*(P1[0] - P2[0]) + P1[2] * (P2[0] - P0[0]) + P2[2] * (P0[0] - P1[0]); 
//	N[2] = P0[0]*(P1[1] - P2[1]) + P1[0] * (P2[1] - P0[1]) + P2[0] * (P0[1] - P1[1]); 
//	float D = P0[0] * (P1[1] * P2[2] - P2[1] * P1[2]) + P1[0] * (P2[1] * P0[2] - P0[1] * P2[2]) + P2[0] * (P0[1] * P1[2] - P1[1] * P0[2]);
//	D = -1.0/D;
//	N[0] *= D;N[1] *= D;N[2] *= D;
//}
//
//
//
//void BackProj2(float P[3], float O[4], float invVP[4][4])
//{
//	O[0] = P[0]*invVP[0][0]+P[1]*invVP[1][0]+P[2]*invVP[2][0]+invVP[3][0]; 
//	O[1] = P[0]*invVP[0][1]+P[1]*invVP[1][1]+P[2]*invVP[2][1]+invVP[3][1]; 
//	O[2] = P[0]*invVP[0][2]+P[1]*invVP[1][2]+P[2]*invVP[2][2]+invVP[3][2]; 
//	O[3] = P[0]*invVP[0][3]+P[1]*invVP[1][3]+P[2]*invVP[2][3]+invVP[3][3]; 
//}
//float3 groundPlane2;
//
//void GetGroundPlane2(float3 &groundPlane, float invVP[4][4])
//{
//	float p0[3]={-1.0, 1.0, 0.0};
//	float p1[3]={ 1.0, 1.0, 0.0};
//	float p2[3]={ 0.0, 0.0, 1.0};
//	float q0[4],q1[4],q2[4],p[3];
//	BackProj2(p0,q0,invVP);q0[3]=1.0/q0[3];q0[0]*=q0[3];q0[1]*=q0[3];q0[2]*=q0[3];
//	BackProj2(p1,q1,invVP);q1[3]=1.0/q1[3];q1[0]*=q1[3];q1[1]*=q1[3];q1[2]*=q1[3];
//	BackProj2(p2,q2,invVP);q2[3]=1.0/q2[3];q2[0]*=q2[3];q2[1]*=q2[3];q2[2]*=q2[3];
//	q0[0]=q0[1]=q0[2]=q1[0]=q1[1]=q1[2]=q2[0]=q2[1]=q2[2]=0;
//	q0[0]=q1[1]=q2[2]=1.0;
//	FitPlane2(q0,q1,q2,p);
//	groundPlane = make_float3(p[0], p[1], p[2]);
//}

void FitPlane(float P0[3], float P1[3], float P2[3], float N[4])
{
	//three point P0,P1,P2 determined a plane with normal N;
	//point are in homogeneous coordinate;
	//plane equation Ax+By+Cz+D=0; if(D!=0) Ax+By+Cz+1=0;
	N[0] = P0[1]*(P1[2] - P2[2]) + P1[1] * (P2[2] - P0[2]) + P2[1] * (P0[2] - P1[2]); 
	N[1] = P0[2]*(P1[0] - P2[0]) + P1[2] * (P2[0] - P0[0]) + P2[2] * (P0[0] - P1[0]); 
	N[2] = P0[0]*(P1[1] - P2[1]) + P1[0] * (P2[1] - P0[1]) + P2[0] * (P0[1] - P1[1]); 
	N[3] = -(P0[0] * (P1[1] * P2[2] - P2[1] * P1[2]) + P1[0] * (P2[1] * P0[2] - P0[1] * P2[2]) + P2[0] * (P0[1] * P1[2] - P1[1] * P0[2]));
	if(fabs(N[3])>1.0e-6)	
	{
		N[3] = 1.0/N[3];
		N[0] *= N[3];N[1] *= N[3];N[2] *= N[3];
		N[3] = 1.0;
	}
}



void BackProj(float P[3], float O[4], float invVP[4][4])
{
	O[0] = P[0]*invVP[0][0]+P[1]*invVP[1][0]+P[2]*invVP[2][0]+invVP[3][0]; 
	O[1] = P[0]*invVP[0][1]+P[1]*invVP[1][1]+P[2]*invVP[2][1]+invVP[3][1]; 
	O[2] = P[0]*invVP[0][2]+P[1]*invVP[1][2]+P[2]*invVP[2][2]+invVP[3][2]; 
	O[3] = P[0]*invVP[0][3]+P[1]*invVP[1][3]+P[2]*invVP[2][3]+invVP[3][3]; 
}


void GetGroundPlane(float4 &groundPlane, float3 &corner, float3 &line, float invVP[4][4])
{
	//float p0[3]={-0.9,-0.9, 0.01};
	//float p1[3]={ 0.9,-0.9, 0.01};
	//float p2[3]={ 0.0, 0.0, 0.6};
	//float q0[4],q1[4],q2[4],p[4];
	//BackProj(p0,q0,invVP);q0[3]=1.0/q0[3];q0[0]*=q0[3];q0[1]*=q0[3];q0[2]*=q0[3];
	//BackProj(p1,q1,invVP);q1[3]=1.0/q1[3];q1[0]*=q1[3];q1[1]*=q1[3];q1[2]*=q1[3];
	//BackProj(p2,q2,invVP);q2[3]=1.0/q2[3];q2[0]*=q2[3];q2[1]*=q2[3];q2[2]*=q2[3];
	float q0[3] = {-5,-3,-5};
	float q1[3] = {5,-3,-5};
	float q2[3] = {-5,-3,5};
	float p[4];
	FitPlane(q0,q1,q2,p);
	//p[0]=p[2]=0;p[1]=1;p[3]=3;
	groundPlane = make_float4(p[0], p[1], p[2], p[3]);
	corner = make_float3(q0[0], q0[1], q0[2]);
	line = make_float3(q0[0]-q1[0], q0[1]-q1[1], q0[2]-q1[2]);
}
